#include <iostream>
#include <string>
#include <deque>
#include <thread>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>


class rosbagProcess
{   
public:
    std::string root_dir = ROOT_DIR;
    const std::string in_bag = std::string(std::string(ROOT_DIR)) + "data/testBag.bag";
    const std::string out_bag = std::string(std::string(ROOT_DIR)) + "data/processBag.bag";
    int reflash = system((std::string("exec rm ") + out_bag).c_str());

    ros::NodeHandle nh;

    std::vector<std::string> Topics;
    std::string leg_odom_Topic = "/leg_odom";
    std::string lidar_odom_Topic = "/Odometry";

    rosbag::Bag inputBag, outputBag;
    
    rosbagProcess()
    {
        inputBag.open(in_bag, rosbag::bagmode::Read);
        outputBag.open(out_bag, rosbag::bagmode::Write);

        Topics.push_back(std::string(leg_odom_Topic));
        Topics.push_back(std::string(lidar_odom_Topic));
    }

    void bagProcessInfo()
    {
        rosbag::View view(inputBag, rosbag::TopicQuery(Topics));
        
        rosbag::View::iterator it = view.begin();
        for (; it != view.end(); ++it)
        {
            auto m = *it;
            std::string thisTopicName = m.getTopic();

            if (thisTopicName == leg_odom_Topic)
            {
                nav_msgs::Odometry::ConstPtr legOdomPrt;
                legOdomPrt = m.instantiate<nav_msgs::Odometry>();

                if (legOdomPrt != NULL)
                {
                    // nav_msgs::Odometry legOdom = *legOdomPrt;
                    outputBag.write(leg_odom_Topic, legOdomPrt->header.stamp, legOdomPrt);
                }
                else
                {
                    ROS_INFO("\033[1;32m No Data of legOdom.\033[0m");
                }
            }

            if (thisTopicName == lidar_odom_Topic)
            {
                nav_msgs::Odometry::ConstPtr lidarOdomPrt;
                lidarOdomPrt = m.instantiate<nav_msgs::Odometry>();

                if (lidarOdomPrt != NULL)
                {
                    // nav_msgs::Odometry lidarOdom = *lidarOdomPrt;
                    outputBag.write(lidar_odom_Topic, lidarOdomPrt->header.stamp, lidarOdomPrt);
                }
                else
                {
                    ROS_INFO("\033[1;32m No Data of lidarOdom.\033[0m");
                }
            }

        }
        inputBag.close();
        outputBag.close();
    }



};






int main(int argc, char** argv)
{ 
    ros::init(argc, argv, "rosbagProcess");

    rosbagProcess MO;

    ROS_INFO("\033[1;32m----> rosbag Procee Started.\033[0m");

    std::thread processBagInfo(&rosbagProcess::bagProcessInfo, &MO);
    processBagInfo.join();

    ROS_INFO("\033[1;32m----> rosbag Procee Finish.\033[0m");


    return 0;
}